//
// Created by Wirano on 2020/11/10.
//

#include "Ano_OPMV_LineTracking_Ctrl.h"
#include "Drv_OpenMV.h"
#include "Ano_OPMV_Ctrl.h"
#include "Ano_Math.h"
#include "Ano_Filter.h"
#include "Ano_Imu.h"
#include "Ano_FlightCtrl.h"

#include "Ano_DT.h"
#include "OMV_2013_Parameter.h"

#include "Nizhou_drv_TOF.h"
#include "Nizhou_drv_T265.h"

//需要调用引用的外部变量：
#define IMU_ROL                 (imu_data.rol)     //横滚角
#define IMU_PIT                 (imu_data.pit)     //俯仰角
#define RELATIVE_HEIGHT_CM      (TOF_height_cm)  //相对高度


#define LT_KP                  U_LT_KP  //比例项
#define LT_KD                  U_LT_KD  //微分项


//需要操作赋值的外部变量：


//===============================
//全局变量：
_ano_opmv_lt_ctrl_st ano_opmv_lt_ctrl; //巡线相关的变量 extern
int16_t line_loss_hold_time; //
float lt_decou_pos_pixel_lpf[2];
uint8_t step_pro_sta; //状态机的步数?
//参数设定：
#define LT_PIXELPDEG    U_LT_PIXELPDEG    //每1角度对应的像素个数，与分辨率和焦距有关，需要调试标定。//3.2f->(160*120,3.6mm)  2.4f->(160*120,2.8mm)
#define LT_CMPPIXEL     U_LT_CMPPIXEL     //每像素对应的地面距离，与焦距和高度有关，需要调试标定。//目前粗略标定
#define LLH_TIME        U_LLH_TIME        //判断目标丢失的保持时间。
#define CONFIRM_TIMES   U_CONFIRM_TIMES   //重确认特征判断的次数，确认次数越多，越能避免误检测，但是也可能导致检测不到。
#define YAW_PAL_DPS     U_YAW_PAL_DPS     //航向角速度，度每秒
#define FORWARD_VEL     U_FORWARD_VEL     //前进速度

/**********************************************************************************************************
*函 数 名: ANO_LTracking_Task
*功能说明: 匿名科创寻线任务
*参    数: 周期时间(ms)
*返 回 值: 无
**********************************************************************************************************/
void ANO_LTracking_Task(uint8_t dT_ms)
{
    //开启控制的条件，可以自己修改
    if (opmv.mode_sta == 2)// && opmv_ct_sta.en)
    {
        //寻线数据旋转解耦合
        ANO_LTracking_Decoupling(&dT_ms, IMU_ROL, IMU_PIT);
        //寻线数据计算
        ANO_LTracking_Calcu(&dT_ms, (int32_t) RELATIVE_HEIGHT_CM);
    } else {
        //reset
        ano_opmv_lt_ctrl.target_loss = 1;
    }
}

/**********************************************************************************************************
*函 数 名: ANO_LTracking_Decoupling
*功能说明: 匿名科创寻线解耦合
*参    数: 周期时间(形参ms)，横滚角度，俯仰角度 //俯仰家对好像没有看到
*返 回 值: 无
**********************************************************************************************************/
static void ANO_LTracking_Decoupling(uint8_t *dT_ms, float rol_degs, float pit_degs)
{
    float dT_s = (*dT_ms) * 1e-3f;

    //有识别到目标
    if (opmv.lt.sta != 0)//(opmv.cb.color_flag!=0)
    {
        //
        ano_opmv_lt_ctrl.target_loss = 0;
        line_loss_hold_time = 0;
    } else {
        //延迟一定时间
        if (line_loss_hold_time < LLH_TIME) {
            line_loss_hold_time += *dT_ms;
        } else {
            //目标丢失标记置位
            ano_opmv_lt_ctrl.target_loss = 1;//目标识别超过了额定时间认为目标丢失
        }
    }
    //
    ano_opmv_lt_ctrl.opmv_pos = -opmv.lt.deviation;

    //仍然识别到目标
    if (opmv.lt.sta != 0) {
        //更新姿态量对应的偏移量
        ano_opmv_lt_ctrl.r2pixel_val = -LT_PIXELPDEG * rol_degs;
        ano_opmv_lt_ctrl.r2pixel_val = LIMIT(ano_opmv_lt_ctrl.r2pixel_val, -50, 50);//宽度100pixel

    } else {
        //姿态量对应偏移量保持不变

    }

    //目标仍然没有丢失
    if (ano_opmv_lt_ctrl.target_loss == 0) //有效，没丢失
    {
        //得到平移偏移量，并低通滤波
        lt_decou_pos_pixel_lpf[0] +=
                0.2f * ((ano_opmv_lt_ctrl.opmv_pos - ano_opmv_lt_ctrl.r2pixel_val) - lt_decou_pos_pixel_lpf[0]);
        //再滤一次
        lt_decou_pos_pixel_lpf[1] += 0.2f * (lt_decou_pos_pixel_lpf[0] - lt_decou_pos_pixel_lpf[1]);

        //赋值 得到的这个变量是
        ano_opmv_lt_ctrl.decou_pos_pixel = lt_decou_pos_pixel_lpf[1];
        //最后得到的像素偏移量会在计算相对地面的高度偏差中使用到
    } else //丢失目标
    {

        //低通复位到0
        LPF_1_(0.2f, dT_s, 0, ano_opmv_lt_ctrl.decou_pos_pixel);


    }
    //
}

/**********************************************************************************************************
*函 数 名: ANO_LTracking_Calcu
*功能说明: 匿名科创寻线计算处理
*参    数: 周期时间(形参ms)，相对高度
*返 回 值: 无
**********************************************************************************************************/
static void ANO_LTracking_Calcu(uint8_t *dT_ms, int32_t relative_height_cm)
{
    static float relative_height_cm_valid;
    static float lt_gnd_pos_err_old;
    //相对高度赋值
    //对相对高度进行一个限幅
    if (relative_height_cm < 500) {
        relative_height_cm_valid = relative_height_cm;
    } else {
        //null
    }
    //记录历史值
    lt_gnd_pos_err_old = ano_opmv_lt_ctrl.ground_pos_err_h_cm;

    //得到地面偏差，单位厘米
    ano_opmv_lt_ctrl.ground_pos_err_h_cm = LT_CMPPIXEL * relative_height_cm_valid * ano_opmv_lt_ctrl.decou_pos_pixel;

    //计算微分偏差，单位厘米每秒
    ano_opmv_lt_ctrl.ground_pos_err_d_h_cmps =
            (ano_opmv_lt_ctrl.ground_pos_err_h_cm - lt_gnd_pos_err_old) * (1000 / (*dT_ms));
    //最后是在距离偏差的控制中使用

}

/**********************************************************************************************************
*函 数 名: ANO_LT_StepProcedure
*功能说明: 匿名科创寻线循迹分步控制任务
*参    数: 周期时间(ms,形参)
*返 回 值: 无
**********************************************************************************************************/

void ANO_LT_StepProcedure(uint8_t *dT_ms)
{
    static uint8_t confirm_cnt;//相当于识别直线时候的置信度
    static int16_t elapsed_time_ms;//已经过去的时间
    switch (step_pro_sta) { //step_pro_sta 文件中的全局变量,状态机的步数?
        case 0: {
            //reset
            ano_opmv_lt_ctrl.exp_yaw_pal_dps = 0;
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[0] = 0;
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[1] = 0;
            elapsed_time_ms = 0;
            step_pro_sta = 1;
        }
            break;
        case 1: {
            //复位确认次数
            confirm_cnt = 0;
            //识别直线
            if (opmv.lt.sta == 1) {
                //
                ano_opmv_lt_ctrl.exp_velocity_h_cmps[0] = FORWARD_VEL;
            }
                //识别到直角左转
            else if (opmv.lt.sta == 2) {
                step_pro_sta = 2;
            }
                //识别到直角右转
            else if (opmv.lt.sta == 3) {
                step_pro_sta = 3;
            } else if (opmv.lt.sta == 0) {
                step_pro_sta = 0;
            }

        }
            break;
        case 2: {
            //确认是否识别到，否则返回
            if (opmv.lt.sta != 2) {
                step_pro_sta = 1;

            } else {
                //再确认n次
                if (confirm_cnt < CONFIRM_TIMES) {
                    confirm_cnt++;
                } else {
                    confirm_cnt = 0;
                    //下一步
                    step_pro_sta = 4;
                }
            }
        }
            break;
        case 3: {
            //确认是否识别到，否则返回
            if (opmv.lt.sta != 3) {
                step_pro_sta = 1;

            } else {
                //再确认n次
                if (confirm_cnt < CONFIRM_TIMES) {
                    confirm_cnt++;
                } else {
                    confirm_cnt = 0;
                    //下一步
                    step_pro_sta = 5;
                }
            }
        }
            break;
        case 4: {
            //刹车减速
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[0] = 10;//识别到直角后减速前进的速度cm/s
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[1] = 0;//左右纠正速度复位
            //持续时间，转弯后偏内测，则加长时间;反之减少时间
            if (elapsed_time_ms < 1500) {
                elapsed_time_ms += *dT_ms;
            } else {
                elapsed_time_ms = 0;
//				if(opmv.lt.sta == 2)
                step_pro_sta = 12;
            }
        }
            break;
        case 5: {
            //刹车减速
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[0] = 10;//识别到直角后减速前进的速度cm/s
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[1] = 0;//左右纠正速度复位
            //持续时间，转弯后偏内测，则加长时间;反之减少时间
            if (elapsed_time_ms < 1500) {
                elapsed_time_ms += *dT_ms;
            } else {
                elapsed_time_ms = 0;
//				if(opmv.lt.sta == 3)
                step_pro_sta = 13;
            }
        }
            break;
        case 12://左转90度
        {
            //
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[0] = 30;//边旋转边给前进速度，走出一定圆弧
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[1] = 0;//左右纠正速度复位
            ano_opmv_lt_ctrl.exp_yaw_pal_dps = -YAW_PAL_DPS;
            //
            if (elapsed_time_ms < 90000 / YAW_PAL_DPS) {
                elapsed_time_ms += *dT_ms;
            } else {
                ano_opmv_lt_ctrl.exp_yaw_pal_dps = 0;
                elapsed_time_ms = 0;
                step_pro_sta = 20;
            }
        }
            break;
        case 13://右转90度
        {
            //
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[0] = 30;//边旋转边给前进速度，走出一定圆弧
            ano_opmv_lt_ctrl.exp_velocity_h_cmps[1] = 0;//左右纠正速度复位
            ano_opmv_lt_ctrl.exp_yaw_pal_dps = YAW_PAL_DPS;
            //
            if (elapsed_time_ms < 90000 / YAW_PAL_DPS) {
                elapsed_time_ms += *dT_ms;
            } else {
                ano_opmv_lt_ctrl.exp_yaw_pal_dps = 0;
                elapsed_time_ms = 0;
                step_pro_sta = 20;
            }
        }
            break;
        case 20://衔接，因旋转后有可能没有看到直线，所以先设定前进500ms
        {
            //
            elapsed_time_ms += *dT_ms;
            //
            if (elapsed_time_ms < 100) {
                elapsed_time_ms += *dT_ms;
            } else if (elapsed_time_ms < 600) {
                elapsed_time_ms += *dT_ms;
                ano_opmv_lt_ctrl.exp_velocity_h_cmps[0] = FORWARD_VEL;
                ano_opmv_lt_ctrl.exp_velocity_h_cmps[1] = 0;
            } else {
                elapsed_time_ms = 0;
                step_pro_sta = 1;
            }
        }
            break;
        default: {

        }
            break;
    }


}

/**********************************************************************************************************
*函 数 名: ANO_LTracking_Ctrl
*功能说明: 匿名科创色块跟踪控制任务
*参    数: 周期时间(ms)
*返 回 值: 无
**********************************************************************************************************/
void ANO_LTracking_Ctrl(uint8_t *dT_ms, uint8_t en)
{
    //开启控制
    if (en) {
        //最终输出到飞行器XY轴坐标上的移动速度,需要根据飞行器对地高度偏差和飞行器对地的高度速度偏差进行计算
        //这一部分数据则是通过ANO_LTracking_Calcu部分获取
        //距离偏差PD控制
        ano_opmv_lt_ctrl.exp_velocity_h_cmps[1]\
 = LT_KP * ano_opmv_lt_ctrl.ground_pos_err_h_cm\
 + LT_KD * ano_opmv_lt_ctrl.ground_pos_err_d_h_cmps;

        //转向修正控制
        if (opmv.lt.sta == 1) {
            //
            ano_opmv_lt_ctrl.exp_yaw_pal_dps = opmv.lt.angle * U_YAW_TURN_DPD;//对于角度的修正DPD是一个复数
            ano_opmv_lt_ctrl.exp_yaw_pal_dps = LIMIT(ano_opmv_lt_ctrl.exp_yaw_pal_dps, -30, 30);
        } else {
            ano_opmv_lt_ctrl.exp_yaw_pal_dps = 0;
        }
        //前进、转向控制
        ANO_LT_StepProcedure(dT_ms);
    } else {
        //
        ano_opmv_lt_ctrl.exp_velocity_h_cmps[0] = 0;
        ano_opmv_lt_ctrl.exp_velocity_h_cmps[1] = 0;
        ano_opmv_lt_ctrl.exp_yaw_pal_dps = 0;
        //
        step_pro_sta = 0;
    }
    //

}	
